/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*LINTLIBRARY*/
/*PROTOLIB1*/

/*#include <math.h>*/
#include "main.h"
#include "conv_cor.h"
#include "rint.h"
#include "calc_cor_eng_stoch.h"
#include "calc_stoch_conv.h"
#include "end_correct_stoch.h"
#include <assert.h>

#if 0  /* now implemented in its own file */
STATIC void CalcStochConv(
fxpt_16 ExcVec[],
fxpt_16 LPImpResp[],
fxpt_16 Conv[MAX_CW_VEC_LEN]);
#endif

#if 0  /* function now in its own file */
STATIC void EndCorrectStoch(
fxpt_16 ExcVal,
fxpt_16 LPImpResp[],
int     LenTruncH,
fxpt_16 Conv[]);
#endif


#if 0  /* function now in its own file */
STATIC void CalcCorEngStoch(
fxpt_16 Residual[RES_LEN],
fxpt_16 Conv[],
fxpt_16 ExcVal1,
fxpt_16 ExcVal2,
int     First,
fxpt_32 *Cor,
fxpt_32 *Energy);
#endif

STATIC void CompGainErrStoch(
fxpt_32 Energy,
fxpt_32 Cor,
fxpt_32 *Gain,
fxpt_32 *Error);

/**************************************************************************
*
* ROUTINE
*               ConvCor
*
* FUNCTION
*               Find codeword gain and error (TERNARY CODE BOOK ASSUMED!)
*
* SYNOPSIS
*               ConvCor(ExcVec, ExVecLen, First, LPImpResp, LenTruncH, NegErr)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ExcVec          fxpt_16  i      excitation vector (ternary codeword)
*       ExcVecLen       int      i      size of ex (dimension of codeword)
*       First           int      i      first call flag
*       LPImpResp       fxpt_16  i      LPC Impulse Response
*       LenTruncH       int      i      Length to truncate impulse response
*       NegErr          fxpt_32  o      negative partial squared error
*
*       ConvCor         fxpt_32 fun     optimal gain for ex
*
*
*==========================================================================
*
* DESCRIPTION
*
* (The calculations below may be valid for version 3.2, but may not be
*  correct for version 3.3).
*
*       For each code word find its gain and error:
*          a.  Filter code words through impulse response
*              of perceptual weighting filter (LPC filter with
*              bandwidth broadening).
*          b.  Correlate filtered result with actual second error
*              signal (e0).
*          c.  Compute MSPE gain and error for code book vector.
*
*       Notes:  Codewords may contain many zeros (i.e., ex(1)=0).  The
*               code book could be accessed by a pointer to nonzero samples.
*               Because the code book is static, it`s silly to test its
*               samples as in the code below.
*
*               Proper selection of the convolution length (len) depends on
*               the perceptual weighting filter's expansion factor (gamma)
*               which controls the damping of the impulse response.
*
*               This is one of CELP's most computationally intensive
*               routines.  Neglecting overhead, the approximate number of
*               DSP instructions (add, multiply, multiply accumulate, or
*               compare) per second (IPS) is:
*
*                      Code book size   MIPS
*                      --------------   ----
*                          64           1.1
*                         128           2.1
*                         256           4.2
*                         512 (max)     8.3
*
*               C:  convolution (recursive truncated end-point correction)
*               R:  correlation
*               E:  energy (recursive end-point correction)
*               G:  gain quantization
*
*       celp code book search complexity (doesn't fully exploit ternary values!):
*       implicit undefined(a-z)
c
c       DSP chip instructions/operation:
*       integer MUL, ADD, SUB, MAC, MAD, CMP
*       parameter (MUL=1)       !multiply
*       parameter (ADD=1)       !add
*       parameter (SUB=1)       !subtract
*       parameter (MAC=1)       !multiply & accumulate
*       parameter (MAD=2)       !multiply & add
*       parameter (CMP=1)       !compare
c
c       CELP algorithm parameters:
*       integer L, len, K, shift, g_bits
*       real p, F
*       parameter (L=60)        !subframe length
*       parameter (len=30)      !length to truncate calculations (<= L)
*       parameter (p=0.77)      !code book sparsity
*       parameter (shift=2)     !shift between code words
*       parameter (K=4)         !number of subframes/frame
*       parameter (F=30.e-3)    !time (seconds)/frame
*       parameter (g_bits=5)    !cbgain bit allocation
c
*       integer j
*       parameter (j=10)
*       integer N(j), i
*       real C, R, E, G, IPS
*       data N/1, 2, 4, 8, 16, 32, 64, 128, 256, 512/
*       print 1
*1      format(10x,'N',10x,'C',13x,'R',12x,'E',15x,'G',13x,'MIPS')
*       do i = 1, j
*          C = (335)*MAD + (N(i)-1)*shift*(1.-p)*len*ADD
*          R = N(i)*L*MAC
*          E = L*MAC + (N(i)-1)*((1.-p*p)*L*MAC + (p*p)*2*MAD)
*          G = N(i)*(g_bits*(CMP+MUL+ADD) + 3*MUL+1*SUB)
*          IPS = (C+R+E+G)*K/F
*          print *,N(i),C*K/1.e6/F,R*K/1.e6/F,E*K/1.e6/F,G*K/1.e6/F,IPS/1.e6
*       end do
*       end
*
*    N        C             R             E               G           MIPS
*    1  8.9333333E-02  8.0000004E-03  8.0000004E-03  2.5333334E-03  0.1078667
*    2  9.1173328E-02  1.6000001E-02  1.1573013E-02  5.0666668E-03  0.1238130
*    4  9.4853334E-02  3.2000002E-02  1.8719040E-02  1.0133334E-02  0.1557057
*    8  0.1022133      6.4000003E-02  3.3011094E-02  2.0266667E-02  0.2194911
*   16  0.1169333      0.1280000      6.1595201E-02  4.0533334E-02  0.3470618
*   32  0.1463733      0.2560000      0.1187634      8.1066668E-02  0.6022034
*   64  0.2052534      0.5120000      0.2330998      0.1621333       1.112487
*  128  0.3230133       1.024000      0.4617727      0.3242667       2.133053
*  256  0.5585334       2.048000      0.9191184      0.6485333       4.174185
*  512   1.029573       4.096000       1.833810       1.297067       8.256450
*
*==========================================================================
*
* CALLED BY
*
*       cbsearch
*
* CALLS
*
*       gainencode2
*
*==========================================================================
*
* REFERENCES
*
*       See REFERENCES in celp.c...and:
*
*       Lin, Daniel, "New Approaches to Stochastic Coding of Speech
*       Sources at Very Low Bit Rates," Signal Processing III:  Theories
*       and Applications (Proceedings of EUSIPCO-86), 1986, p.445.
*
*       Xydeas, C.S., M.A. Ireton and D.K. Baghbadrani, "Theory and
*       Real Time Implementation of a CELP Coder at 4.8 and 6.0 kbits/s
*       Using Ternary Code Excitation," Fifth International Conference on
*       Digital Processing of Signals in Communications, 1988, p. 167.
*
*       Ess, Mike, "Simple Convolution on the Cray X-MP,"
*       Supercomputer, March 1988, p. 35
*
*       Supercomputer, July 1988, p. 24
*
***************************************************************************/

/*
 *   STARIUM NOTES:  This routine used to spend a lot of time shifting elements
 *                   "conv" array.  We added EXTRA_CONV_SPACE to the Conv array
 *                   so we could just shift our position around in the array
 *                   rather than shifting elements over.
 */
#define EXTRA_CONV_SPACE  ((STOCH_CB_SIZE * 2) - 2)

fxpt_32 ConvCor(
fxpt_16 ExcVec[],                       /* 15.0 format */
int     First,
fxpt_16 LPImpResp[SF_LEN],              /* 2.13 format */
fxpt_16 Residual[RES_LEN],              /* 15.0 format */
fxpt_32 *NegErr)
{
        static fxpt_16  Conv[MAX_CW_VEC_LEN + EXTRA_CONV_SPACE];        /* 15.0 format */
        static fxpt_32  Energy=0;       /* 30.0 format */
        fxpt_32         Cor;            /* 30.0 format */
        fxpt_32         Gain;           /* 27.4 format */

        static fxpt_16 *Conv_pos;

FXPT_PUSHSTATE("ConvCor", -1.0, -1.0);
        if (First) {

                ASSERT( Conv_pos == 0  ||  Conv_pos == &Conv[0] );
                Conv_pos = Conv + EXTRA_CONV_SPACE;

                /* For first codeword, calculate and save convolution
                 * of codeword with truncated impulse response
                 */
                CalcStochConv(ExcVec, LPImpResp, Conv_pos);
        }
        else {

                /* End correct the convolution sum on subsequent code words */
                /* First Shift */
                EndCorrectStoch(ExcVec[1], LPImpResp, Conv_pos);
                Conv_pos--;

                /* Second Shift */
                EndCorrectStoch(ExcVec[0], LPImpResp, Conv_pos);
                Conv_pos--;
        }

        /* Calculate correlation and energy */
        CalcCorEngStoch(Residual, Conv_pos, ExcVec[0], ExcVec[1], First, &Cor,
            &Energy);

        /* Compute Gain and Error */
        CompGainErrStoch(Energy, Cor, &Gain, NegErr);

FXPT_POPSTATE();
        return Gain;
}

/**************************************************************************
*
* ROUTINE
*               CalcStochConv
*
* FUNCTION
*               Calculate and save convolution of codeword with
*               truncated impulse response
*
* SYNOPSIS
*               CalcStochConv(ExcVec, ExcVecLen, LPImpulseResponse, Conv)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ExcVec          fxpt_16  i      excitation vector (ternary codeword)
*       ExcVecLen       int      i      excitation vector length
*       LPImpResp       fxpt_16  i      LPC Impulse Response
*       LenTruncH       int      i      length to truncate impulse response
*       Conv            fxpt_16  o      Convolution of codeword with impulse
*                                       response
*
***************************************************************************
*          For first code word, calculate and save convolution
*          of code word with truncated (to len) impulse response:
*
*          NOTES: A standard convolution of two L point sequences
*                 produces 2L-1 points, however, this convolution
*                 generates only the first L points.
*
*                 A "scalar times vector accumulation" method is used
*                 to exploit (skip) zero samples of the code words:
*
*                 min(L-i+1, len)
*          y         =  SUM  ex * h   , where i = 1, ..., L points
*           i+j-1, t    j=1    i   j
*
*                       ex |1 2  .  .  .  L|
*          h |x x len ... 2 1|               = y(1)
*            h |x x len ... 2 1|             = y(2)
*                             :                 :
*                        h |x x len ... 2 1| = y(L)
*
***************************************************************************/
#if 0  /* now implemented in its own file */
void CalcStochConv(
fxpt_16 ExcVec[],                       /* 15.0 format */
fxpt_16 LPImpResp[],                    /* 2.13 format */
fxpt_16 Conv[MAX_CW_VEC_LEN])           /* 15.0 format */
{
        int     i, j;

FXPT_PUSHSTATE("CalcStochConv", -1.0, -1.0);
        /* Initialize */
        for (i=0; i< SF_LEN; i++) {
                Conv[i] = 0;
        }

        for (i=0; i< SF_LEN; i++) {
                if (ExcVec[i] != 0) {
                        for(j=0; j<min(SF_LEN - i, LEN_TRUNC_H); j++) {
                                /*Conv[i+j] += ExcVec[i] * LPImpResp[j];*/
                                Conv[i+j] = fxpt_add16(Conv[i+j],
                                    fxpt_mult16_round(ExcVec[i], LPImpResp[j],
                                    13));
                        }
                }
        }
FXPT_POPSTATE();
}
#endif


/**************************************************************************
*
* ROUTINE
*               EndCorrectStoch
*
* FUNCTION
*               End correct the convolution sum
*
* SYNOPSIS
*               EndCorrectStoch(ExcVal, LPImpResp, LenTruncH, Conv)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ExcVal          fxpt_16  i      Excitation value
*       LPImpResp       fxpt_16  i      LPC impulse response
*       LenTruncH       int      i      Length to truncate impulse response
*       Conv            fxpt_16  o      Convolution sum
*
***************************************************************************
*
*          End correct the convolution sum on subsequent code words:
*          (Do two end corrections for a shift by 2 code book)
*
*          y     =  0
*           0, 0
*          y     =  y        + ex * h   where i = 1, ..., L points
*           i, m     i-1, m-1   -m   i  and   m = 1, ..., cbsize-1 code words
*
*          NOTE:  The data movements in loops "do 59 ..." and "do 69 ..."
*                 are performed many times and can be quite time consuming.
*                 Therefore, special precautions should be taken when
*                 implementing this.  Some implementation suggestions:
*                 1.  Circular buffers with pointers to eliminate data moves.
*                 2.  Fast "block move" operation as offered on some DSPs.
*
*
***************************************************************************/
#if 0 /* function now in it's own file */
void EndCorrectStoch(
fxpt_16 ExcVal,                         /* 15.0 format */
fxpt_16 LPImpResp[],                    /* 2.13 format */
fxpt_16 Conv[])                         /* 15.0 format */
{
        int     i;

FXPT_PUSHSTATE("EndCorrectStoch", -1.0, -1.0);
        if (ExcVal != 0) {

                /* Ternary stochastic code book (-1, 0, +1) */
                if (ExcVal == 1) {
                        for (i=LEN_TRUNC_H-1; i>0; i--) {
                                /*Conv[i-1] += LPImpResp[i];*/
                                Conv[i-1] = fxpt_add16(Conv[i-1],
                                    fxpt_shr16_round(LPImpResp[i], 13));
                        }
                }
                else {
                        for(i=LEN_TRUNC_H-1; i>0; i--) {
                                /*Conv[i-1] -= LPImpResp[i];*/
                                Conv[i-1] = fxpt_sub16(Conv[i-1],
                                    fxpt_shr16_round(LPImpResp[i], 13));
                        }
                }
        }

        for(i=SF_LEN-1; i>0; i--) {
                Conv[i] = Conv[i-1];
        }
        /*Conv[0] = ExcVal*LPImpResp[0];*/
        Conv[0] = fxpt_mult16_round(ExcVal, LPImpResp[0], 13);
FXPT_POPSTATE();
}
#endif

/**************************************************************************
*
* ROUTINE
*               CalcCorEngStoch
*
* FUNCTION
*               Calculate correlation and energy
*
* SYNOPSIS
*               CalcCorEngStoch(Residual, Conv, ExcVal1, ExcVal2, First, Cor, Energy)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       Residual        fxpt_16  i      Spectrum and adaptive residual
*       Conv            fxpt_16  i      Convolution sum
*       ExcVal1         fxpt_16  i      First excitation vector value
*       ExcVal2         fxpt_16  i      Second excitation vector value
*       First           int      i      First subframe flag
*       Cor             fxpt_32  o      Correlation
*       Energy          fxpt_32  o      Energy
*
***************************************************************************
*
*       Calculate correlation and energy:
*       e0 = spectrum & pitch prediction residual
*       y  = error weighting filtered code words
*
*       \/\/\/  CELP's computations are focused in this correlation \/\/\/
*               - For a 512 code book this correlation takes 4 MIPS!
*               - Decimation?, Down-sample & decimate?, FEC codes?
*
***************************************************************************/
#if 0 /* function now in it's own file */
void CalcCorEngStoch(
fxpt_16 Residual[RES_LEN],              /* 15.0 format */
fxpt_16 Conv[],                         /* 15.0 format */
fxpt_16 ExcVal1,                        /* 15.0 format */
fxpt_16 ExcVal2,                        /* 15.0 format */
int     First,
fxpt_32 *Cor,                           /* 30.1 format */
fxpt_32 *Energy)                        /* 30.1 format */
{
        int i;
        static fxpt_16 NextLastConv=0;
        static fxpt_16 LastConv=0;

FXPT_PUSHSTATE("CalcCorEngStoch", -1.0, -1.0);
        /*  Initialize */
        *Cor = 0;

        /*  Calculate correlation */
        for (i=0; i<SF_LEN; i++) {
                /**Cor += Conv[i]*Residual[i];*/
                *Cor = fxpt_mac32(*Cor, Conv[i], Residual[i]);
        }

        /*  End correct energy on subsequent code words */
        if ((ExcVal1 == 0) && (ExcVal2 == 0) && (!First)) {
                /**Energy -= NextLastConv*NextLastConv + LastConv*LastConv;*/
                *Energy = fxpt_sub32(*Energy, fxpt_add32(
                    fxpt_mult32_fast(NextLastConv, NextLastConv),
                    fxpt_mult32_fast(LastConv, LastConv)));
        }
        else {
                *Energy = 0;
                for (i=0; i<SF_LEN; i++) {
                        /**Energy += Conv[i] * Conv[i];*/
                        *Energy = fxpt_mac32(*Energy, Conv[i], Conv[i]);
                }
        }
        NextLastConv = Conv[SF_LEN-2];
        LastConv = Conv[SF_LEN-1];
FXPT_POPSTATE();
}
#endif


/**************************************************************************
*
* ROUTINE
*               CompGainErrStoch
*
* FUNCTION
*               Compute gain and error
*
* SYNOPSIS
*               CompGainErrStoch(Energy, Cor, Gain, Error)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       Energy          fxpt_32  i      Energy
*       Cor             fxpt_32  i      Correlation
*       Gain            fxpt_16  o      Codebook Gain
*       Error           fxpt_32  o      Codebook Error
*
***************************************************************************
*
*       Compute gain and error:
*         NOTE: Actual MSPE = e0.e0 - gain(2*cor-gain*eng)
*               since e0.e0 is independent of the code word,
*               minimizing MSPE is equivalent to maximizing:
*                    match = gain(2*cor-gain*eng)
*               If unquantized gain is used, this simplifies:
*                    match = cor*gain
*
*       Independent (open-loop) quantization of gain and match (index):
*
***************************************************************************/

void CompGainErrStoch(
fxpt_32 Energy,                 /* 30.0 format */
fxpt_32 Cor,                    /* 30.0 format */
fxpt_32 *Gain,                  /* 27.4 format */
fxpt_32 *Error)                 /* 30.1 format */
{
FXPT_PUSHSTATE("CompGainErrStoch", -1.0, -1.0);
        if (Energy == 0)
                *Gain = Cor;
        else
                /**Gain = Cor/Energy;*/
                *Gain = fxpt_div32(Cor, Energy, 4);

        /**Error = Cor * *Gain;*/
//        *Error = fxpt_mult64_round(Cor, *Gain, 4);
        *Error = fxpt_mult64_round_good(Cor, *Gain, 3);
FXPT_POPSTATE();
}
